#pragma once

#include <string>
#include <unordered_map>
#include <fstream>
#include <iostream>

#include <cstdint>

#include <nlohmann/json.hpp>

#include "point.hpp"
#include "path.hpp"

namespace dw
{
    class Agent : 
        public std::enable_shared_from_this<Agent>
    {
    public:
        enum class Shape : uint32_t
        {
            Circle,
            Rectangle,
            House
        };

        struct State
        {
            ID id = 0;

            Shape shape = Shape::Circle;
            double dRadius = 0.5;

            double dOmega = 0;
            double dVelocity = 0;
            double dAcceleration = 0;

            Dir vDirection;
            Coord vPosition;

            operator bool() const
            {
                return Vec({0, 0}) != this->vDirection;
            }

            friend void to_json(nlohmann::json &j, const Agent::State &agent);
            friend void from_json(const nlohmann::json &j, Agent::State &agent);

            std::string encode() const {
                nlohmann::json j;
                to_json(j["agent"], *this);
                return j.dump();
            }

            Agent::State &decode(const std::string &data_msg) {
                nlohmann::json j = nlohmann::json::parse(data_msg);
                from_json(j["agent"], *this);
                return *this;
            }

            friend std::ostream &operator<<(std::ostream &os, const Agent::State &agent)
            {
                os << "Agent ID: " << agent.id << std::endl;
                os << "   shape: " << uint32_t(agent.shape) << std::endl;
                os << "  radius: " << agent.dRadius << std::endl;
                os << "       v: " << agent.dVelocity << std::endl;
                os << "       a: " << agent.dAcceleration << std::endl;
                os << "     dir: " << agent.vDirection << std::endl;
                os << "       w: " << agent.dOmega << std::endl;
                os << "     pos: " << agent.vPosition << std::endl;

                return os;
            }
        };

    protected:
        bool b_Updated;
        Agent::State state;

    public:
        // Empty Constructor
        // Construct an agent at (0,0), facing +x, shape circle, Radius = 0.5
        // v = 0, d = 0, a = 0, w = 0
        Agent(ID id = 0) {
            this->b_Updated = false;

            this->state.id = id;
            this->state.shape = Shape::Circle;
            this->state.dRadius = 0.5;

            this->state.dOmega = 0;
            this->state.dVelocity = 0;
            this->state.dAcceleration = 0;
            this->state.vDirection = {1, 0};

            this->state.vPosition = {0, 0};
        }

        // Copy Constructor
        Agent(const Agent& agent, ID id = 0) {
            this->b_Updated = false;
            this->state = agent.state;

            if (id) this->state.id = id;
            this->setDirection(agent.state.vDirection.norm());
        }

        // Construct from Agent::State
        Agent(const Agent::State& agent_state, ID id = 0) {
            this->b_Updated = false;
            this->state = agent_state;

            if (id) this->state.id = id;
            this->setDirection(agent_state.vDirection.norm());
        }

        // Default Deconstructor
        virtual ~Agent() { /* done */ }

        operator bool() const
        {
            return bool(this->state);
        }

    public:
        // save as file
        virtual bool save(const std::string& FilePath) {
            std::ofstream ofs(FilePath);
            if (!ofs.is_open()) return false;

            ofs << this->encode();
            ofs.close();
            return true;
        }

        // load from file
        virtual bool load(const std::string& FilePath) {
            std::ifstream ifs(FilePath);
            if (!ifs.is_open()) return false;
            
            std::string encoded;
            ifs >> encoded;
            ifs.close();

            return decode(encoded);
        }

        friend void to_json(nlohmann::json &j, const Agent &agent);
        friend void from_json(const nlohmann::json& j, Agent& agent);

        // encode to json
        std::string encode() {
            using json = nlohmann::json;

            json root;
            json& agent = root["agent"];
            to_json(agent, *this);

            return root.dump();
        }

        // decode from json
        Agent& decode(const std::string& encoded) {
            using json = nlohmann::json;

            json root = json::parse(encoded);
            json agent = root["agent"];
            from_json(agent, *this);

            return *this;
        }

        friend std::ostream &operator<<(std::ostream &os, const Agent &agent) {
            os << agent.state;
            return os;
        }

        // get Agent::State
        const Agent::State& getAgentState() const
        {
            return state;
        }

        Agent::State getAgentState()
        {
            return state;
        }

    public:
        // set agent state
        Agent& set(const Agent::State& agent_state, ID id = 0) {
            this->state = agent_state;

            if (id) this->state.id = id;
            this->setDirection(agent_state.vDirection.norm());
            return *this;
        }

        // set id
        Agent& setID(ID id) {
            this->state.id = id;
            return *this;
        }

        // set shape
        Agent& setShape(Shape shape) {
            this->state.shape = shape;
            return *this;
        }

        Agent &setShape(uint32_t shape)
        {
            if (shape >= 0 && shape < 3) {
                this->state.shape = Shape(shape);
            }
            else {
                this->state.shape = Shape::Circle;
            }
            return *this;
        }
        
        // set position
        Agent &setPosition(const Vec &pos) {
            this->state.vPosition = pos;
            return *this;
        }

        // set Direction
        Agent &setDirection(const double angle)
        {
            this->state.vDirection = Dir({1.0f, angle}).cart();
            this->state.vDirection = this->state.vDirection.norm();
            return *this;
        }

        Agent &setDirection(const Vec &dir) {
            if (Vec{0,0} == dir) {
                this->state.vDirection = dir;
            } else {
                this->state.vDirection = dir.norm();
            }
            return *this;
        }

        // set Velocity
        Agent &setVelocity(const double v) {
            if (v >= 0) {
                state.dVelocity = v;
            } else {
                state.dVelocity = -v;
                state.vDirection = -state.vDirection;
            }
            return *this;
        }

        Agent &setVelocity(const Vec &v) {
            state.dVelocity = v.mag();
            state.vDirection = v.norm();
            return *this;
        }

        // set Acceleration
        Agent &setAcceleration(const double a){
            state.dAcceleration = a;
            return *this;
        }
        
        Agent &setAcceleration(const Vec &a) {
            state.dAcceleration = a.dot(state.vDirection);
            state.dOmega = a.dot(state.vDirection.perp());
            return *this;
        }

        // set Angular Velocity
        Agent &setAngularVelocity(const double w) {
            state.dOmega = w;
            return *this;
        }

        Agent &Stop() {
            this->setVelocity(0);
            this->setAcceleration(0);
            this->setAngularVelocity(0);
            return *this;
        }

    public:
        ID getID() const {
            return state.id;
        }

        double getRadius() const {
            return state.dRadius;
        }

        Shape getShape() const {
            return state.shape;
        }

        double getVelocity() const {
            return state.dVelocity;
        }

        Vec getvVelocity() const {
            return state.dVelocity * state.vDirection;
        }

        double getAcceleration() const {
            return state.dAcceleration;
        }

        Vec getvAcceleration() const {
            return state.dAcceleration * state.vDirection // a * dt
                 + state.dOmega * state.dVelocity * state.vDirection.perp(); // w*dt*v
        }

        Dir getDirection() const {
            return state.vDirection;
        }

        double getDirectionRad() {
            Coord polar = state.vDirection.polar();
            return polar.y;
        }

        Coord getPosition() const {
            return state.vPosition;
        }
    
        double getAngularVelocity() const {
            return state.dOmega;
        }

    protected:
        virtual void updateVelocity(double fTime) {
            Vec v = getvVelocity() + fTime * getvAcceleration();
            state.dVelocity = v.mag();
            state.vDirection = v.norm();
        }

        virtual void updatePosition(double fTime) {
            state.vPosition += getvVelocity() * fTime;
        }

    public:
        // update agent state by agent state
        Agent& Update(const Agent::State& state) {
            b_Updated = true;
            return this->set(state);
        }

        // update agent state by time
        Agent &Update(double fTime)
        {
            if (!b_Updated) {
                updateVelocity(fTime);
                updatePosition(fTime);
            }
            return *this;
        }
    };

    class AutonomousAgent : public Agent
    {
    private:
        std::shared_ptr<Path> p_Path;
    public:
        AutonomousAgent() : Agent() { /* done */ }
        AutonomousAgent(ID id) : Agent(id) { /* done */ }
        AutonomousAgent(const AutonomousAgent& agent, const int id = 0) {
            this->b_Updated = false;
            this->state = agent.state;
            if (id) this->state.id = id;
            this->p_Path = nullptr;
        }

        void follow(std::shared_ptr<Path> p) {
            p_Path = p;
        }

    private:
        virtual void updateVelocity(double fTime) override
        {
            if (nullptr == p_Path || p_Path->empty())
            {
                this->Stop();
                return;
            }

            Coord then = p_Path->current();
            Coord now = p_Path->next();
            if (now == then) {
                this->Stop();
                return;
            }
            
            Vec vNow = (now - then) / fTime;
            Vec vThen = this->getvVelocity();
            Vec a = (vNow - vThen) / fTime;
            this->setAcceleration(a);
            this->setVelocity(vNow);
        }

        virtual void updatePosition(double fTime) override
        {
            this->setPosition(p_Path->current());
        }
    };


    void to_json(nlohmann::json &j, const Agent::State &state)
    {
        Coord dir = state.vDirection;
        dir = dir.polar();
        double dir_angle = dir.y;

        j = nlohmann::json{
            {"id", state.id},
            {"shape", state.shape},
            {"radius", state.dRadius},
            {"direction", dir_angle},
            {"velocity", state.dVelocity},
            {"angularVelocity", state.dOmega},
            {"acceleration", state.dAcceleration},
            {"position", {
                {"x", state.vPosition.x},
                {"y", state.vPosition.y}
                }
            }
        };
    }

    void from_json(const nlohmann::json &j, Agent::State &state)
    {
        if (j.contains("direction") && !j["direction"].is_null() &&
            j.contains("velocity") && !j["velocity"].is_null() &&
            j.contains("angularVelocity") && !j["angularVelocity"].is_null() &&
            j.contains("position") && !j["position"].is_null() &&
            j["position"].contains("x") && !j["position"]["x"].is_null() &&
            j["position"].contains("y") && !j["position"]["y"].is_null() &&
            j.contains("id") && !j["id"].is_null() &&
            j.contains("shape") && !j["shape"].is_null() &&
            j.contains("radius") && !j["radius"].is_null())
        {
            double angle = j["direction"];
            state.vDirection = Coord(1, angle).cart();

            state.id = j["id"];
            state.shape = Agent::Shape(j["shape"]);
            state.dRadius = j["radius"];
            state.dVelocity = j["velocity"];
            state.dOmega = j["angularVelocity"];
            state.dAcceleration = j["acceleration"];
            state.vPosition = Coord({j["position"]["x"], j["position"]["y"]});
        } else {
            if (j.contains("id") && !j["id"].is_null()) {
                state.id = j["id"];
            } else {
                state.id = 0;
            }
            state.shape = Agent::Shape::Circle;
            state.dRadius = 0;
            state.dVelocity = 0;
            state.vDirection = Vec(0, 0);
            state.dOmega = 0;
            state.dAcceleration = 0;
            state.vPosition = Coord(-1, -1);
        }
    }

    void to_json(nlohmann::json &j, const Agent &agent)
    {
        Agent::State state = agent.state;
        to_json(j, state);
    }

    void from_json(const nlohmann::json &j, Agent &agent)
    {
        Agent::State state;
        from_json(j, state);
        agent = Agent(state);
    }
}
